Uncertainty-Constrained Differential Dynamic Programming in Belief Space for Vision Based Robots

Most mobile robots follow a modular sense-plan-act system architecture that can lead to poor performance or even catastrophic failure for visual inertial navigation systems due trajectories devoid of feature matches. Planning in belief space provides unified approach tightly couple the perception, planning and control modules, leading are robust noisy measurements disturbances. However, existing methods handle uncertainties as costs require manual tuning varying environments hardware. We therefore propose novel trajectory optimization formulation incorporates inequality constraints on uncertainty Augmented Lagrangian based stochastic differential dynamic programming method space. Furthermore, we develop probabilistic visibility model accounts discontinuities limits. Our simulation tests demonstrate our different environments, holonomic nonholonomic motion models with no involved. also show improved model.